using BepuPhysics;
using BepuPhysics.Collidables;
using BepuPhysics.CollisionDetection;
using BepuPhysics.Constraints.Contact;
using BepuUtilities;
using BepuUtilities.Collections;
using BepuUtilities.Memory;
using DemoContentLoader;
using DemoRenderer;
using DemoRenderer.UI;
using DemoUtilities;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
using System;
#endif

namespace Demos.Demos
{
    /// <summary>
    /// 演示如何使用接触访问器和ISolverContactDataExtractor枚举求解器中的接触约束。
    /// </summary>
    public class SolverContactEnumerationDemo : Demo
    {
        BodyHandle sensorBodyHandle;
        public override void Initialize(ContentArchive content, Camera camera)
        {
            camera.Position = new Vector3(0, 5, 25);
            camera.Yaw = 0;
            camera.Pitch = 0;

            // PositionFirstTimestpper是最简单的时步模式,但由于它在帧开始时将速度积分到位置中,因此直接修改了时间步长之外的速度
            // 将在碰撞检测或解算器有机会介入之前集成。这在这个演示中很好。其他内置选项包括PositionLastTimestpper和SubsteppingTimestpper。
            // 请注意,timeSteper还具有回调,您可以使用这些回调在处理阶段之间执行逻辑,如BeforeCollisionDetection。
            Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());

            // 在传感器顶部放置一个棱锥体,以便有更多的触点可供查看。
            var boxShape = new Box(1, 1, 1);
            boxShape.ComputeInertia(1, out var boxInertia);
            var boxIndex = Simulation.Shapes.Add(boxShape);

            const int rowCount = 20;
            for (int rowIndex = 0; rowIndex < rowCount; ++rowIndex)
            {
                int columnCount = rowCount - rowIndex;
                for (int columnIndex = 0; columnIndex < columnCount; ++columnIndex)
                {
                    Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(
                        (-columnCount * 0.5f + columnIndex) * boxShape.Width,
                        (rowIndex + 0.5f) * boxShape.Height + 10, 0),
                        boxInertia,
                        new CollidableDescription(boxIndex, 0.1f),
                        new BodyActivityDescription(0.01f)));
                }
            }


            sensorBodyHandle = Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 0, 1), 10, Simulation.Shapes, new Box(4, 2, 6)));

            // 在传感器下面放一张网,这样就可以显示非凸形触点。
            const int planeWidth = 128;
            const int planeHeight = 128;
            DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight,
                (int x, int y) =>
                {
                    return new Vector3(x - planeWidth / 2, 1 * MathF.Cos(x / 2f) * MathF.Sin(y / 2f), y - planeHeight / 2);
                }, new Vector3(2, 1, 2), BufferPool, out var planeMesh);
            Simulation.Statics.Add(new StaticDescription(new Vector3(0, -2, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2),
                new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f)));
        }

        struct Contact
        {
            public Vector3 OffsetA;
            public float Depth;
            // 出于演示的目的,我们将为每个单独的接触存储一条法线,即使原始流形可能是凸形的(因此在其所有接触之间共享一条法线)。
            public Vector3 Normal;
            public float PenetrationImpulse;
            // 我们还将根据凸性从不同来源导出摩擦冲量-凸流形具有单一扭曲/切线摩擦约束,而非凸流形具有每接触切线摩擦。
            public float FrictionImpulseMagnitude;
        }

        struct ConstraintContacts
        {
            public QuickList<Contact> Contacts;
            public BodyHandle BodyA;
            // 对于一个实体约束,此值将为-1。
            public BodyHandle BodyB;

            public ConstraintContacts(BufferPool pool, BodyHandle a, BodyHandle b)
            {
                // 非凸流形的数量永远不会小于凸数,因此我们将为非凸流形预先分配足够的空间。
                Contacts = new QuickList<Contact>(NonconvexContactManifold.MaximumContactCount, pool);
                BodyA = a;
                BodyB = b;
            }

            public ConstraintContacts(BufferPool pool, BodyHandle a) : this(pool, a, new BodyHandle(-1)) { }
        }

        struct Extractor : ISolverContactDataExtractor
        {
            // 我们将把求解器数据转换成不同的形式--只是一个联系人列表,其中包含每个联系人的基本数据。
            // 您存储的内容取决于您的应用程序的需要-并没有说明您必须使用此布局。
            // 可以将ISolverContactDataExtractor视为构建其他(更方便的)抽象的基础。
            public QuickList<ConstraintContacts> ConstraintContacts;
            public BufferPool Pool;

            public Extractor(BufferPool pool, int initialCapacity)
            {
                Pool = pool;
                ConstraintContacts = new QuickList<ConstraintContacts>(initialCapacity, pool);
            }

            // 回调区分凸和非凸,因为底层数据不同。
            // 每个接触处的非凸流形可以有不同的法线,而凸流形在所有接触上共享一条法线。
            // 这也会影响累积的脉冲-由于潜在的不同法线,非凸形脉冲需要包括每个接触的切线摩擦脉冲。

            // 在以下所有代码中,您可能会注意到一种不寻常的模式-例如prestep.GetNormal(ref prestep.)。
            // 要绕过这样一个事实,这有点像是一种巧妙的做法
            // 1)结构成员函数不能返回对"this"实例的引用,并且
            // 2)当前版本的C#没有办法要求接口需要静电函数。
            // 这是另一个为了最小开销而进行的奇怪的小折衷。

            void ExtractConvexData<TPrestep, TAccumulatedImpulses>(ref ConstraintContacts constraintContacts, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
                where TPrestep : struct, IConvexContactPrestep<TPrestep>
                where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses<TAccumulatedImpulses>
            {
                // 请注意,我们将原始的矢量化引用缩小为更便于应用程序的AOS表示。
                Vector3Wide.ReadFirst(prestep.GetNormal(ref prestep), out var normal);

                // 我们将通过将共享摩擦脉冲分配给按触点穿透冲量加权的触点来近似每个触点的摩擦力。
                float totalPenetrationImpulse = 0;
                for (int i = 0; i < prestep.ContactCount; ++i)
                {
                    ref var sourceContact = ref prestep.GetContact(ref prestep, i);
                    ref var targetContact = ref constraintContacts.Contacts.AllocateUnsafely();
                    Vector3Wide.ReadFirst(sourceContact.OffsetA, out targetContact.OffsetA);
                    // 我们可以使用[0]来访问插槽,因为我们的前一步捆绑内存引用已经偏移。
                    targetContact.Depth = sourceContact.Depth[0];
                    targetContact.Normal = normal;
                    targetContact.PenetrationImpulse = impulses.GetPenetrationImpulseForContact(ref impulses, i)[0];
                    totalPenetrationImpulse += targetContact.PenetrationImpulse;
                }
                Vector2Wide.ReadFirst(impulses.GetTangentFriction(ref impulses), out var tangentFriction);
                var twistFriction = impulses.GetTwistFriction(ref impulses)[0];
                // 这不是对冲动的"正确"分配,我们只是想要一个粗略的感觉。
                var frictionMagnitudeApproximation = MathF.Sqrt(tangentFriction.LengthSquared() + twistFriction * twistFriction);
                var impulseScale = totalPenetrationImpulse > 0 ? frictionMagnitudeApproximation / totalPenetrationImpulse : 0;
                for (int i = 0; i < prestep.ContactCount; ++i)
                {
                    ref var contact = ref constraintContacts.Contacts[i];
                    contact.FrictionImpulseMagnitude = contact.PenetrationImpulse * impulseScale;
                }
            }

            public void ConvexOneBody<TPrestep, TAccumulatedImpulses>(BodyHandle bodyHandle, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
                where TPrestep : struct, IConvexContactPrestep<TPrestep>
                where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses<TAccumulatedImpulses>
            {
                ref var constraintContacts = ref ConstraintContacts.Allocate(Pool);
                constraintContacts = new ConstraintContacts(Pool, bodyHandle);
                ExtractConvexData(ref constraintContacts, ref prestep, ref impulses);
            }

            public void ConvexTwoBody<TPrestep, TAccumulatedImpulses>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
                where TPrestep : struct, ITwoBodyConvexContactPrestep<TPrestep>
                where TAccumulatedImpulses : struct, IConvexContactAccumulatedImpulses<TAccumulatedImpulses>
            {
                ref var constraintContacts = ref ConstraintContacts.Allocate(Pool);
                constraintContacts = new ConstraintContacts(Pool, bodyHandleA, bodyHandleB);
                ExtractConvexData(ref constraintContacts, ref prestep, ref impulses);
            }

            void ExtractNonconvexData<TPrestep, TAccumulatedImpulses>(ref ConstraintContacts constraintContacts, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
                where TPrestep : struct, INonconvexContactPrestep<TPrestep>
                where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses<TAccumulatedImpulses>
            {
                // 非凸型不需要摩擦力的近似,我们可以直接从求解结果中拉出。
                for (int i = 0; i < prestep.ContactCount; ++i)
                {
                    ref var sourceContact = ref prestep.GetContact(ref prestep, i);
                    ref var targetContact = ref constraintContacts.Contacts.AllocateUnsafely();
                    Vector3Wide.ReadFirst(sourceContact.Offset, out targetContact.OffsetA);
                    targetContact.Depth = sourceContact.Depth[0];
                    Vector3Wide.ReadFirst(sourceContact.Normal, out targetContact.Normal);

                    ref var contactImpulses = ref impulses.GetImpulsesForContact(ref impulses, i);
                    targetContact.PenetrationImpulse = contactImpulses.Penetration[0];
                    Vector2Wide.ReadFirst(contactImpulses.Tangent, out var tangentImpulses);
                    targetContact.FrictionImpulseMagnitude = tangentImpulses.Length();
                }
            }

            public void NonconvexOneBody<TPrestep, TAccumulatedImpulses>(BodyHandle bodyHandle, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
                where TPrestep : struct, INonconvexContactPrestep<TPrestep>
                where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses<TAccumulatedImpulses>
            {
                ref var constraintContacts = ref ConstraintContacts.Allocate(Pool);
                constraintContacts = new ConstraintContacts(Pool, bodyHandle);
                ExtractNonconvexData(ref constraintContacts, ref prestep, ref impulses);
            }

            public void NonconvexTwoBody<TPrestep, TAccumulatedImpulses>(BodyHandle bodyHandleA, BodyHandle bodyHandleB, ref TPrestep prestep, ref TAccumulatedImpulses impulses)
                where TPrestep : struct, ITwoBodyNonconvexContactPrestep<TPrestep>
                where TAccumulatedImpulses : struct, INonconvexContactAccumulatedImpulses<TAccumulatedImpulses>
            {
                ref var constraintContacts = ref ConstraintContacts.Allocate(Pool);
                constraintContacts = new ConstraintContacts(Pool, bodyHandleA, bodyHandleB);
                ExtractNonconvexData(ref constraintContacts, ref prestep, ref impulses);
            }

            public void Dispose()
            {
                for (int i = 0; i < ConstraintContacts.Count; ++i)
                {
                    ref var constraintContacts = ref ConstraintContacts[i];
                    constraintContacts.Contacts.Dispose(Pool);
                }
                ConstraintContacts.Dispose(Pool);
            }
        }

        public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font)
        {
            var sensorBody = Simulation.Bodies.GetBodyReference(sensorBodyHandle);
            var extractor = new Extractor(BufferPool, sensorBody.Constraints.Count);
            // 触点提取器背后的基本思想是将其提交给能够理解求解器布局的窄相位触点存取器,
            // 然后,它将针对遇到的约束类型调用联系人提取器的相关回调。
            // 在这里,我们将枚举当前影响传感器主体的所有约束,尝试从每个约束中提取联系数据。
            // 如果有不是接触约束的约束,它们将被跳过。
            for (int i = 0; i < sensorBody.Constraints.Count; ++i)
            {
                Simulation.NarrowPhase.TryExtractSolverContactData(sensorBody.Constraints[i].ConnectingConstraintHandle, ref extractor);
            }
            // 我们现在已经提取了联系信息。我们来分析一下吧！
            // 出于演示的目的,我们将在触点绘制调试形状以表示不同的属性。
            int sensorContactCount = 0;
            for (int manifoldIndex = 0; manifoldIndex < extractor.ConstraintContacts.Count; ++manifoldIndex)
            {
                ref var constraintContacts = ref extractor.ConstraintContacts[manifoldIndex];
                var bodyA = Simulation.Bodies.GetBodyReference(constraintContacts.BodyA);
                sensorContactCount += constraintContacts.Contacts.Count;
                for (int contactIndex = 0; contactIndex < constraintContacts.Contacts.Count; ++contactIndex)
                {
                    ref var contact = ref constraintContacts.Contacts[contactIndex];
                    var contactPosition = contact.OffsetA + bodyA.Pose.Position;
                    Matrix3x3 basisPose;
                    // 我们想要可视化摩擦和穿透脉冲,所以圆柱体是一个很好的选择-半径和长度。
                    // 长度将沿接触法线定向,而半径将沿切线方向扩展。
                    BepuPhysics.Helpers.BuildOrthnormalBasis(contact.Normal, out basisPose.X, out basisPose.Z);
                    basisPose.Y = contact.Normal;
                    var baseLength = 0.2f;
                    var baseRadius = 0.1f;
                    // 我们将进行纯粹的投机性接触(负深度)、固定的最小尺寸和不同的颜色。
                    var contactVisualShape = contact.Depth < 0 ?
                        new Cylinder(baseRadius, baseLength) :
                        new Cylinder(baseRadius + MathF.Min(5, contact.FrictionImpulseMagnitude * 0.1f), baseLength + MathF.Min(5, contact.PenetrationImpulse * 0.3f));
                    RigidPose contactVisualPose;
                    contactVisualPose.Position = contactPosition + contact.Normal * contactVisualShape.HalfLength;
                    QuaternionEx.CreateFromRotationMatrix(basisPose, out contactVisualPose.Orientation);
                    renderer.Shapes.AddShape(contactVisualShape, Simulation.Shapes, ref contactVisualPose, contact.Depth < 0 ? new Vector3(0, 0, 1) : new Vector3(0, 1, 0));
                }
            }
            renderer.TextBatcher.Write(text.Clear().Append("Sensor manifold constraint count: ").Append(extractor.ConstraintContacts.Count).Append(", contact count: ").Append(sensorContactCount), new Vector2(32, 32), 20, Vector3.One, font);

            extractor.Dispose();
            base.Render(renderer, camera, input, text, font);
        }
    }
}
